#ifndef SORT_HPP
#define SORT_HPP

#include <memory.h>
#include <algorithm>

struct dummy_counter
{
    dummy_counter & operator += (size_t /* n */) { }
};

/* Sorts an array of elements in the range [first, last) according to their
 * hash value mapped by the function 'hash', and stores the results in a 
 * separate array 'dest'. The sorting algorithm is stable.
 */
template <class FwdIt, class RanIt, class Func>
void insertion_sort(FwdIt first, FwdIt last, RanIt dest, Func hash)
{
    if (first == last)
        return;

    *dest = *first;
    size_t i = 1;
    for (FwdIt it = first; ++it != last; i++)
    {
        size_t k = hash(*it);
        size_t hole = i;
        while (hole > 0 && hash(dest[hole-1]) > k)
            hole--;

        for (size_t j = i; j > hole; j--)
            dest[j] = dest[j - 1];
        dest[hole] = *it;
    }
}

/* Sorts an array of elements in-place according to the predicate 'less'.
 * Returns the inversion number as a by-product. The sorting is stable.
 */
template <class Ty, class Pred, class Counter>
Counter insertion_sort(Ty dest[], size_t n, Pred less, Counter inv_num)
{
    for (size_t i = 1; i < n; i++)
    {
        Ty val = dest[i];
        size_t j;
        for (j = i; j > 0 && less(val, dest[j-1]); j--)
        {
            dest[j] = dest[j-1];
        }
        if (j != i)
        {
            dest[j] = val;
            inv_num += (i - j);
        }
    }
    return inv_num;
}

/* Sorts an array of elements in-place according to the predicate 'less'.
 * Returns the inversion number as a by-product. The sorting is stable.
 */
template <class Ty, class Pred>
void insertion_sort(Ty dest[], size_t n, Pred less)
{
    insertion_sort(dest, n, less, dummy_counter());
}

template <class T>
struct digit_extractor
{
    size_t pos;
    digit_extractor(size_t k) : pos(k) { }
    size_t operator()(const T& x) const { return x[pos]; }
};

/* Sorts an array of elements in the range [first, last) according to their
 * hash value mapped by the function 'hash', and stores the results in a
 * separate array 'dest'. The hash value must be an unsigned integer in the
 * range [0, ValueSize). The number of elements with each hash value is stored
 * in the array 'count'. This sorting algorithm is stable.
 */
template <size_t ValueSize, class FwdIt, class RanIt, class size_type, class Func>
void stable_counting_sort(
    FwdIt first,
    FwdIt last,
    RanIt dest,
    size_type count[ValueSize], 
    Func hash, 
    bool zeroed = false)
{
    typedef size_t key_type;
    
    if (!zeroed)
        memset(count, 0, sizeof(size_type) * ValueSize);

    size_type n = 0;
    for (FwdIt it = first; it != last; ++it, ++n)
    {
        key_type k = hash(*it);
        ++count[k];
    }

    /* Use insertion sort when there are only a few elements. */
    if (n <= 4)
    {
        insertion_sort(first, last, dest, hash);
        return;
    }

    size_type start[ValueSize];
    size_type index = n;
    for (key_type k = ValueSize; k > 0; )
    {
        --k;
        start[k] = (index -= count[k]);
    }

    for (FwdIt it = first; it != last; ++it)
    {
        key_type k = hash(*it);
        dest[start[k]++] = *it;
    }
}

/* Sort an array of elements [first, last) according to their hash value
 * mapped by the function 'hash'. The hash value must be an unsigned integer
 * in the range of [0, ValueSize). The number of elements with each hash
 * value is stored in the array 'count'. This sorting algorithm is NOT stable.
 */
template <size_t ValueSize, class T, class size_type, class Func>
void counting_sort(
    T first[],
    size_type n,
    size_type count[ValueSize], 
    Func hash, 
    bool zeroed = false)
{
    typedef size_t key_type;

    if (!zeroed)
        memset(count, 0, sizeof(size_type) * ValueSize);

    for (size_type i = 0; i < n; i++)
    {
        key_type k = hash(first[i]);
        ++count[k];
    }

#if 0
    /* current[k] = current index of bucket (k-1); current[0] = 0 */
    size_type current[ValueSize+1];
    size_type n = last - first;

    /* Find the start index of each bucket, and find the smallest bucket. */
    key_type bucket;
    size_type index = n;
    for (bucket = ValueSize; bucket != 0 && index != 0; bucket--)
        current[bucket] = (index -= count[bucket-1]);
    current[0] = 0;

    /* Move the elements into their respective buckets. */
    size_type end = 0;  /* end index of the current bucket (exclusive) */
    for (size_type i = 0; i < n; )
    {
        key_type k = hash(first[i]);

        while (current[k] >= end) /* not in the correct bucket */
        {
            std::swap(first[i], first[current[k+1]++]);
            k = hash(first[i]);
        }
        if (current[k] == i)
            current[k]++;
        i = current[k];
#if 0
        while (k != bucket) // tbd: wrong
        {
            // start[k] > i

            std::swap(first[i], first[current[k]++]);
            k = hash(first[i]);
        }
        if (current[k] == i)
            current[k]++;
        i = current[k];
#endif
    }
#endif

    size_type start[ValueSize], current[ValueSize];
    size_type index = 0;
    for (size_t k = 0; k < ValueSize; k++)
    {
        current[k] = start[k] = index;
        index += count[k];
    }

    for (size_type i = 0; i < n; )
    {
        size_t k = hash(first[i]);
        while (start[k] > i)
        {
            std::swap(first[i], first[current[k]++]);
            k = hash(first[i]);
        }
        if (current[k] == i)
            current[k]++;
        i = current[k];
    }
}

template <class RanIt, class Pred>
void quick_sort(RanIt first, RanIt last, Pred less_than)
{
    size_t n = last - first;
    if (n <= 1)
        return;

    /* Use the last element as pivot. */
    size_t pivot = n - 1;

    /* Partition the array. */
    size_t j = 0;
    for (size_t i = 0; i < n - 1; i++)
    {
        if (!less_than(first[pivot], first[i])) /* a[i] <= a[pivot] */
        {
            if (i != j)
                std::swap(first[i], first[j]);
            j++;
        }
    }
    if (j != pivot)
        std::swap(first[j], first[pivot]);

    /* Sort each sub-array. */
    quick_sort(first, first + j, less_than);
    quick_sort(first + j + 1, last, less_than);
}

/* Merges two arrays according to predicate 'less' and stores the result in
 * a separate array. Returns the inversion number as a by-product.
 */
template <class FwdIt1, class FwdIt2, class OutIt, class Pred, class Counter>
Counter merge(FwdIt1 first1, FwdIt1 last1, FwdIt2 first2, FwdIt2 last2, 
              OutIt dest, Pred less, Counter inv_num)
{
    FwdIt1 it1 = first1;
    FwdIt2 it2 = first2;
    OutIt it = dest;
    size_t inv_cnt = 0;
    while (it1 != last1 && it2 != last2)
    {
        if (less(*it2, *it1))
        {
            *it = *it2;
            ++it2;
            inv_cnt++;
        }
        else
        {
            *it = *it1;
            ++it1;
            inv_num += inv_cnt;
        }
        ++it;
    }
    for (; it1 != last1; ++it1, ++it)
    {
        *it = *it1;
        inv_num += inv_cnt;
    }
    for (; it2 != last2; ++it2, ++it)
    {
        *it = *it2;
    }
    return inv_num;
}

/* Sorts an array of elements in-place according to the predicate 'less'. An
 * auxiliary buffer of the same size as data must be provided. The contents
 * of the auxiliary buffer upon return is undefined. The sorting is stable.
 * Returns the inversion number as a by-product.
 */
template <class Ty, class Pred, class Counter>
Counter merge_sort(Ty data[], size_t n, Pred less, Ty aux[], Counter num_inv)
{
    /* Use insertion sort to sort small blocks in-place. */
    const size_t BlockSize = 4;
    for (size_t i = 0; i < n; i += BlockSize)
    {
        size_t m = std::min(BlockSize, n - i);
        num_inv = insertion_sort(data + i, m, less, num_inv);
    }

    /* Recursively merge small blocks. */
    Ty *input = data;
    Ty *output = aux;
    for (size_t width = BlockSize; width < n; width *= 2)
    {
        for (size_t i = 0; i < n; i += width * 2)
        {
            Ty *start = input + i;
            Ty *mid = input + std::min(i + width, n);
            Ty *end = input + std::min(i + width * 2, n);
            Ty *out = output + i;
            if (mid == end)
                std::copy(start, mid, out);
            else
                num_inv = merge(start, mid, mid, end, out, less, num_inv);
        }
        std::swap(input, output);
    }

    /* Make sure we output to the data[] buffer. */
    std::swap(input, output);
    if (output != data)
        std::copy(output, output + n, data);

    return num_inv;
}

#endif /* SORT_HPP */
